/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "MatchStorage.h"
using namespace std;
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
/// Generic Quicksort for vectors
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
template <class T> unsigned int pivot (vector<T> & v, unsigned int start, unsigned int stop, unsigned int position)
	// partition vector into two groups
	// values smaller than or equal to pivot
	// values larger than pivot
	// return location of pivot element
{
		// swap pivot into starting position
	swap (v[start], v[position]);

		// partition values
	unsigned int low = start + 1;
	unsigned int high = stop;
	while (low < high)
		if (v[low] < v[start])
			low++;
	else if (v[--high] < v[start])
		swap (v[low], v[high]);

		// then swap pivot back into place
	swap (v[start], v[--low]);
	return low;
}

template <class T> void quickSort(vector<T> & v, unsigned int low, unsigned int high){
	// no need to sort a vector of zero or one elements
		if (low >= high) return;

	// select the pivot value
		unsigned int pivotIndex = (low + high) / 2;

	// partition the vector
		pivotIndex = pivot (v, low, high, pivotIndex);

	// sort the two sub vectors
		if (low < pivotIndex)
				quickSort(v, low, pivotIndex);
		if (pivotIndex < high)
				quickSort(v, pivotIndex + 1, high);
}
template <class T> void quickSort(vector<T> & v){
		unsigned int numberElements = v.size ();
		if (numberElements > 1) 
				quickSort(v, 0, numberElements - 1);
}
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////

/**
 * Costructor requires the number of poses maintained in this storage
 * @param N_ number of matches
 */
CMatchStorage::CMatchStorage(int N_){
	N=N_;
	worst = 0;
	worstW = 0;
}

/**
 * Costructor for Net packet input (see setNetPacket())
 * @param *in the pointer to input memory block
 * @param size size of input memory block
 */
CMatchStorage::CMatchStorage(const char *in,unsigned int size){
	setNetPacket(in,size);
}
		
/**
* Update a new pose match result into the storage
*/
void CMatchStorage::update(TPoseMatch &pm){
	
	if(storage.size()<N){    ///< Still filling the storage
		storage.push_back(pm);
		if(pm.weight>worstW){
			worstW = pm.weight; worst = storage.size()-1; 
		}
		return;
	}
	
	if(pm.weight<worstW){  ///< if better than worst
		//fprintf(stderr,"Updating weight (%.2f @ %d) to %2f -- ",worstW,worst,pm.weight);
		storage[worst] = pm; ///< replace the worst value
		//fprintf(stderr," w=%.2f -- ",storage[worst].weight);
		searchWorstIndex();  ///< Update the worst case values
	}

}

/**
* Tests if given weight @w is better than worse
* @param w the weight to be tested
* @return true if it was, else false
*/
bool CMatchStorage::isBetter(float w){
	if(storage.size()<N) return true;
	if(w<worstW) return true;
	return false;
}
		
/**
* Sorts the storage
**/
void CMatchStorage::sort(){
	quickSort(storage);
	///GLUDGE 
	if(storage.size()>1){
		if(storage[storage.size()-1].weight < storage[storage.size()-2].weight){
			vector<TPoseMatch> tmp = storage; ///< stupid copy
			storage.clear(); 									///< clear this
			unsigned int i=0;
			while(tmp[i].weight < tmp[tmp.size()-1].weight){
				storage.push_back(tmp[i]);
				i++;
			}
			storage.push_back(tmp[tmp.size()-1]);
			for(unsigned int j=i;j<tmp.size()-1;j++) storage.push_back(tmp[j]);
			
			//fprintf(stderr,"SWAP (%d, %.3f) to (%d, %3f)\n",i,storage[i].weight, storage.size()-1,storage[storage.size()-1].weight);
			
			//swap(storage[i],storage[storage.size()-1]);	
		}
	} 
}

/**
* Provides the storage as a (char *) memory block
* @param *out a pointer to memory containing storage. if NULL the memory is reserved, else the size must be (N*SIZE_OF_TPOSEMATCH)
* @return number of bytes in *out 
*/
unsigned int CMatchStorage::getNetPacket(char **out){
	if(*out == NULL){
		*out = (char *) malloc(SIZE_OF_TPOSEMATCH*storage.size()); ///< malloc data
	}
	///Cpy data
	for(unsigned int i=0;i<storage.size();i++){
		memcpy((void *)(*out+i*SIZE_OF_TPOSEMATCH),(void *) &storage[i],SIZE_OF_TPOSEMATCH);
	}
	
	return (SIZE_OF_TPOSEMATCH*storage.size());
}
/**
* Sets a char* memory block into a storage format
* @param *in the pointer to memory that contains the data
* @param size the size of data, must be N*SIZE_OF_TPOSEMATCH
* @return true if ok, else false
*/
bool CMatchStorage::setNetPacket(const char *in, unsigned int size){
	if(in == NULL || size == 0 || size%SIZE_OF_TPOSEMATCH!=0){
			fprintf(stderr,"CMatchStorage::setNetPacket:Invalid input parameters (size='%d')!\n",size);
			return false;
	}
	
	storage.clear(); ///< Empty if there was something
	N = size/SIZE_OF_TPOSEMATCH;
	
	for(unsigned int i=0;i<N;i++){
		TPoseMatch pm;
		memcpy((void *)&pm,(void *)(in+i*SIZE_OF_TPOSEMATCH),SIZE_OF_TPOSEMATCH );
		storage.push_back(pm);
	}
	
	searchWorstIndex();
	return true;
}

void CMatchStorage::searchWorstIndex(){
	if(storage.size()<N) worst = storage.size(); ///< storage not full yet
	else{
		worstW = -1;
		worst=0;
		for(unsigned int i=0;i<N;i++){
			if(storage[i].weight > worstW){
				worst = i;
				worstW = storage[i].weight;
			}
		}
		//fprintf(stderr," New worst = %.2f@%d\n",worstW,worst);
	}
}

